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[57] ABSTRACT 

The described and improved multi-arm invention of this 


application presents three strategies for adaptive con- 
trol of cooperative multi-arm robots which coordinate 
control over a common load. In the position-position 
control strategy, the adaptive controllers ensure that 
the end-effector positions of both arms track desired 
trajectories in Cartesian space despite unknown time- 
varying interaction forces exerted through a load. In 
the position-hybrid control strategy, the adaptive con- 
troller of one arm controls end-effector motions in the 
free directions and applied forces in the constraint di- 
rections; while the adaptive controller of the other arm 
ensures that the end-effector tracks desired position 
trajectories. In the hybrid-hybrid control strategy, the 
adaptive controllers ensure that both end-effectors 
track reference position trajectories while simulta- 
neously applying desired forces on the load. In all three 
control strategies, the cross-coupling effects between 
the arms are treated as “disturbances” which are com- 
pensated for by the adaptive controllers while follow- 
ing desired commands in a common frame of reference. 
The adaptive controllers do not require the complex 
mathematical model of the arm dynamics or any knowl- 
edge of the arm dynamic parameters or the load param- 
eters such as mass and stiffness. Circuits in the adaptive 
feedback and feedforward controllers are varied by 
novel adaptation laws. 
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propose a method for control of dual-arm robots in a 
DUAL-ARM MANIPULATORS WITH ADAPTIVE master/slave manner. T. Ishida, Force Control in Coordi- 
CONTROL nation of Two Arms, Proc. 5th Intern. Conf. on Artificial 

Intelligence, pp. 717-722, 1977, considers parallel and 


ORIGIN OF THE INVENTION 5 

The invention described herein was made in the per- 
formance of work under a NASA contract and is sub- 
ject to the provisions of Public Law 96517 (35 USC 202) 
in which the contractor has elected to retain title. 

This application is a continuation-in-part of a pending 
application assigned to the same assignee and identified 
as a METHOD AND APPARATUS FOR ADAPT- 
IVE FORCE AND POSITION CONTROL OF MA- 
NIPULATORS, filed on Apr. 6, 1987 having Ser. No. 
07/035,061 now U.S. Pat. No. 4,860,215. 15 

BACKGROUND OF THE INVENTION 

1. Field of the Invention 

This invention relates to control systems for robotic 
manipulators and more particularly relates to adaptive 20 
control for a plurality of robotic arms, e.g. a multi-arm 
system in which the cross-coupling between robotic 
arms through a common load is treated as though it 
were an externally caused disturbance. 

2. Description of the Prior Art 25 

An adaptive control system invented by this inventor 

is described and claimed in the above-identified applica- 
tion and a considerable number of publications are set 
forth therein, which publications are incorporated 
herein by reference as though they were set forth ex- 30 
pressly herein. The prior art and references cited 
therein are called to the attention of the Patent Office as 
being of background relevance to this invention. As of 
this filing date, the identified application has received an 
Office Action and the following patents have been 35 
cited; Horack U.S. Pat. No. 4,547,858, Koyama et al 
U.S. Pat. No. 4,580,229; Sugimoto et al U.S. Pat. No. 
4,621,332; and Osuka U.S. Pat. No. 4,725,942. These 
patents do not have any significant relevance to the 
invention herein described and claimed. 40 

The system of the identified application is improved 
upon by being extended from a single arm approach to 
a dual-arm (or higher) approach with a novel method 
and apparatus being described for overcoming the 
cross-coupling that exists in a common load being ma- 45 
nipulated by independently controlled manipulators, or 
arms, in a multi-arm system. Additional background 
material relevant to the development and a fuller under- 
standing of this invention is given in the following para- 
graphs. 50 

During the past decade, robot manipulators (“arms”) 
have been utilized in industry for performing simple 
tasks, and it is foreseen that in the near future anthropo- 
morphic robots will replace human operators in carry- 
ing out various complex tasks both in industry and in 55 
hazardous environments. Nevertheless, present-day 
robots can be considered at best as “handicapped” oper- 
ators due to their single-arm structure. It is evident that 
a multiplicity of robot arms yields greater dexterity and 
increased efficiency and provides capability of handling 60 
larger loads. Dual-arm robots will therefore have capa- 
bilities which may match those of ambidextrous human 
operators in dexterity and efficiency. 

The research on dual-arm robots is at its early stages 
at the present time and a few approaches are currently 65 
available. E. Nakano et al., Cooperational Control of the 
Anthropomorphous Manipulator MELARM, Proc. 4th 
Intern. Conf. on Industrial Robots, pp. 251-260, 1974, 


rotational transfer of loads using dual-arm robots. S. 
Fujii et al., Coordinated Computer Control of a Pair of 
Manipulators, Proc. 4th World Congress on Theory of 
Machines and Mechanisms, pp. 411-417, Newcastle- 
upon-Tyne, England, 1975, suggest a technique for 
dual-arm control based on the method of virtual refer- 
ence. C. Alford et al., Coordinated Control of Two Robot 
Arms, Proc. Intern. Conf. on Robotics, pp. 468-473, 
Atlanta, Ga., 1984, describe a method for coordinated 
control of two arms. Y. Zheng et al., Constrained Rela- 
tions Between Two Coordinated Industrial Robots, Proc. 
Machine Intelligence Conf., Rochester, N.Y., 1985 and 
Computation of Input Generalized Forces for Robots with 
Closed Kinematic Chain Mechanisms, IEEE Journal of 
Robotics and Automation, pp. 95-103, Vol. RA-1, No. 2, 
1985, obtain constrained relations and control laws for 
two coordinated arms. T. Tarn et al., Coordinated Con- 
trol of Two Robot Arms, Proc. IEEE Intern. Conf. on 
Robotics and Automation, pp. 1 193—202, San Fran- 
cisco, Calif., 1986, employ the “Global” linearization 
technique for dual-arm control. S. Hayati, Hybrid Posi- 
tion/Force Control of Multi-Arm Cooperating Robots, 
Proc. IEEE Intern. Conf. on Robotics and Automation, 
pp. 82-89, San Francisco, Calif., 1986, and in pending 
U.S. patent application Method and Apparatus for Hy- 
brid Position /Control of Multi-Arm Cooperating Robot, 
filed Mar. 21, 1988, Ser. No. 06/845,991 proposes a 
method for controlling dual-arm robots based on parti- 
tioning the load between the arms. A. Koivo, Adaptive 
Position-Velocity-Force Control of Two Manipulators, 
Proc. 24th IEEE Conf. on Decision and Control, pp. 
1529-1532, Ft. Lauderdale, Fla., 1985, suggests an 
adaptive control technique for dual-arm robots using 
the self-tuning approach. J. Lim et al., On a Control 
Scheme for Two Cooperating Robot Arms, Proc. 24th 
IEEE Conf. on Decision and Control, pp. 334-337, Ft. 
Lauderdale Fla., 1985, describes a positional control 
scheme for two cooperating robot arms. 

Some recent results of single-arm adaptive control 
are reported in the following papers: H. Seraji, Adaptive 
Control of Robotic Manipulators, JPL Engineering 
Memorandum 347-182, January, 1986; H. Seraji, Direct 
Adaptive Control of Manipulators in Cartesian Space, 
Journal of Robotic Systems, February, 1987 (to appear); 
and H. Seraji, Adaptive Forces and Position Control of 
Manipulators, JPL Engineering Memorandum 347-192, 
October, 1986. 

The above-identified articles, to the extent that they 
are properly considered prior art, do not teach or sug- 
gest a dual-arm adaptive control system, nor such a 
system having adaptive hybrid control of each arm 
independently. Moreover, only in this application is it 
taught that a multi-arm adaptive control system is reli- 
ably operable so long as the load’s inter-arm cross-cou- 
pling is treated as though that cross-coupling were an 
externally caused disturbance. The adaptive hybrid 
control system of this invention can compensate for that 
cross-coupling because of the novel force and/or posi- 
tion control laws as herein defined. 

There are certain key differences between my single- 
arm invention’s approach and the conventional hybrid 
control approach of Raibert and Craig, referred to 
above. Firstly, in my single-arm invention, the force or 
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position control problems are formulated in the Carte- 
sian space with the end-effector Cartesian forces as the 
manipulated variables; whereas in Raibert and Craig, 
the problems are formulated in the joint space. The 
single-arm invention’s formulation results in computa- 5 
tional improvement since inverse Jacobians are not 
required for the controllers’ operation. Secondly, the 
single-arm invention’s hybrid system operates on the 
measured variables so as to produce the position and 
force variables that need to be controlled; whereas in 10 
Raibert and Craig, a selection matrix and its comple- 
ment are used after formulation of tracking errors. In 
summary, the Raibert and Craig disclosure simply does 
not teach or suggest the novel concepts of my inven- 
tion. 15 

SUMMARY OF THE INVENTION 

My invention of the parent application (“single-arm” 
invention) discloses a novel method and apparatus for 
the design of adaptive force and position controllers 20 
within a hybrid control architecture. The hybrid con- 
troller includes an adaptive force controller which 
achieves tracking of desired force setpoints, and an 
adaptive position controller which accomplishes track- 
ing of desired position trajectories. Force and position 25 
controller gains are varied by novel adaptation laws. 
These controllers are computationally fast and suitable 
for on-line implementation with high sampling rates and 
such adaptive gains are implemented in feedback and 
feedforward controllers. These controllers are capable 30 
of compensating for dynamic cross-couplings that exist 
between position and force control loops. 

A key feature of my single-arm invention is that my 
adaptive hybrid controller architecture does not require 
knowledge of the complex dynamic model or parameter 35 
values of the manipulator or the environment. The 
force and position controllers are linear and stable and 
generate real-time signals which vary and compensate 
for system non-linearities in order to achieve a desired 
position/force response. 40 

An additionally attractive feature of my single-arm 
adaptive controllers is an ability to compensate for dy- 
namic cross-couplings that exist between the position 
and force control loops in the hybrid control architec- 
ture. Furthermore, the adaptive force and position con- 45 
trailers have “learning capabilities” to cope with unpre- 
dictable changes in the manipulator or environment 
parameters such as environment or robotic arm stiff- 
ness. This is due to the fact that the controller gains are 
adapted rapidly on the basis of the manipulator’s Carte- 50 
sian space performance. Low computational require- 
ments make the control loops of the single-arm inven- 
tion suitable for implementation in on-line hybrid con- 
trol with high sampling rates. 

The described and improved multi-arm invention of 55 
this application presents three strategies for adaptive 
control of cooperative dual-arm robots. In the position- 
position control strategy, the adaptive controllers en- 
sure that the end-effector positions of both arms track 
desired trajectories in Cartesian space despite unknown 60 
time-varying interaction forces exerted through the 
load. In the position-hybrid control strategy, the adapt- 
ive controller of one arm controls end-effector motions 
in the free directions and applied forces in the constraint 
directions; while the adaptive controller of the other 65 
arm ensures that the end-effector tracks desired position 
trajectories. In the hybrid-hybrid control strategy, the 
adaptive controllers ensure that both end-effectors 
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track reference position trajectories while simulta- 
neously applying desired forces on the load. 

In all three control strategies, the cross-coupling 
effects between the arms are treated as “disturbances” 
which are rejected, or compensated for, by the adaptive 
controllers while following desired commands in a 
common frame of reference. The adaptive controllers 
do not require the complex mathematical model of the 
arm dynamics or any knowledge of the arm dynamic 
parameters or the load parameters such as mass and 
stiffness. The controllers have simple structures and are 
computationally fast for on-line implementation with 
high sampling rates. Simulation results are given to 
illustrate the proposed adaptive control strategies. 

BRIEF DESCRIPTION OF THE DRAWING 

FIG. 1 depicts a block diagram of an improved hy- 
brid control architecture in keeping with this invention; 

FIG. 2 is a simplified block diagram of a controller 
for performing adaptive force control in accordance 
with the invention; 

FIG. 3 is a simplified block diagram of a controller 
for performing adaptive position control in accordance 
with the invention; 

FIG. 4 is a block diagram of a basic module which is 
repeated as necessary for implementation of the control- 
ler functions of adaptive force and position control- in 
accordance with this invention; 

FIG. 5 depicts a tri-level hierarchal control for two, 
or higher-numbered arm, robotic systems; 

FIGS. 6 and 7 depict, respectively a single-arm and a 
cooperative dual-arm manipulator; 

FIG. 8 depicts a simplified load diagram that is useful 
in developing a better understanding of the invention; 

FIG. 9, including FIGS. 9a, 9b, 9c, 9 d, 9e, and 9f 
depict the results of a computer simulation in adaptive 
position tracking which are useful in performance eval- 
uation of the invention; 

FIG. 10, including FIGS. 10a, 106, 10c, 10 d, and 10c, 
depict the results of a computer simulation in adaptive 
position-hybrid control which are useful in perfor- 
mance evaluation of the invention; and 

FIG. 11, including 11a, 116, 11c and lid, depicts the 
results of a computer simulation in adaptive hybrid- 
hybrid control which are useful in performance evalua- 
tion of the invention. 

DESCRIPTION OF THE PREFERRED 
EMBODIMENT 

The description of the invention claimed in this appli- 
cation will first include a summary of the novel single- 
arm force and position control laws and then those laws 
will be developed and applied to a multi-arm system of 
this invention. Incorporated herein by reference, in 
accordance with §608.01(p) of the Manual of Patent 
Examining Procedure, from the above-identified single- 
arm application, is the mathematical formulation of the 
adaptive force and/or position control laws as set forth 
in section 1.1, page 10 through section 3, page 28. More 
particularly such incorporation includes Equations (21) 
through (24) for the gains of the PID controller of FIG. 
3 hereof in the implementation of the linear adaptive 
force control law given by Equations (25) and (26): and 
likewise the formulation and implementation of the 
linear adaptive position control law of Equation (37) 
and the controller gains of Equations (38) through (43). 

To avoid confusion, the Equations of this continua- 
tion-inpart application are numbered starting with 
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Equation no. 101, etc. The exact same force and posi- 
tion control laws apply to multi-arm hybrid adaptive 
control as apply to single-arm hybrid adaptive control 
except the mathematical notation is changed slightly. It 
will be readily apparent to those of ordinary skill in this 5 
art, however, that the Equations are the same and one 
of the keys to the inventive features of this application 
is noting that the cross-coupling terms, C p , Equation 
(25) and C/ Equation (37) in the adaptive position and 
force loops of a single-arm system, also compensates for '0 
the cross-coupling that the independently controlled 
arm of one controller introduces into another indepen- 
dently controlled arm in a dual or higher-numbered 
multi-arm system. Stated in a slightly different way the 
cross-coupling term which results from an environment 
in the single-arm disclosure is replaced by the coupling 
through the load that is being held by a second arm in 
the multi-arm disclosure. Otherwise, the two systems 
are the same and the analysis for one system is fully and 
completely applicable to the other system. 

The blanket generality noted above may be appreci- 
ated by a brief reference to FIG. 6 in comparison to 
FIG. 7. FIG. 6 depicts a common way of showing a 
robot manipulator 600 having at least two joint-con- ^ 
nected sections 600A, 600B and an end effector 605. 
The manipulator 600’s end-effector 605 is moveable in a 
Cartesian space 630 and may come into contact with 
another object or a wall, such as environment 610. In 
FIG. 7, two arms 600 and 660 are shown and the envi- 
ronment in this instance is replaced by common load 
610' that is being manipulated by and between the two 
arms, 600 and 660, respectively. 

In order to show the correspondence with my earlier 
invention, I have identified the common load by num- 35 
ber 610' so that its correspondence to the wall, or other 
object 610 will be more readily apparent. Although not 
depicted, common reference systems, independent con- 
trollers with synchronization at the trajectory generator 
level and common coordination (at the INTERMEDI- 40 
ATE LEVEL, FIG. 5) is to be understood throughout 
the disclosure as will become readily apparent by the 
following detailed description. 

The presentation of the detailed description given 
hereinafter in this application is structured as follows. In 45 
Section 1 (“position-position”), the position-position 
control strategy is discussed and documented by simula- 
tion results. In Section 2 (“position-hybrid”), the posi- 
tion-hybrid, e.g. position-force control strategy is devel- 
oped and demonstrated by a numerical example. The 50 
hybrid-hybrid, force/position, or vice-versa, control 
strategy is addressed in Section 3 (“hybrid-hybrid”) and 
an illustrative example is given. The remainder of this 
application discusses the results of the foregoing multi- 
arm presentation and draws some conclusions. 55 

SECTION 1-POSITION-POSITION 

In this section, we shall investigate the first control 
strategy for dual-arm manipulators in which both arms 
are in pure position control, as shown in FIG. 2. In 60 
other words, the positions and orientations of both end- 
effectors are required to track desired trajectories in a 
common frame of reference. In this situation, uncon- 
trolled forces and torques will be exerted on the com- 
mon load 610' held by the end effectors 605,665 of the 65 
dual arms such as those represented in the LOW 
LEVEL section of FIG. 5. Investigated herein is the 
performance of the adaptive position control law of my 
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system in the face of the interaction forces and torques 
exerted through the load 610'. 

The dynamic model of each manipulator arm (arm 
600 or 660, FIG. 7) can be represented by a differential 
equation in Cartesian space as was observed by O. Kha- 
tib, in Dynamic Control of Maniulators in Cartesian 
Space. Proc. 6th IFToMM Congress on Theory of Ma- 
chines and Mechanisms, pp. 1128-1131, New Delhi, 
India, 1983. 

M(X)X+N(X.X) + G{X) + H(X)±f=F (101) 

where the above terms are defined as: 


x,x,x 

= nxl vectors of end-effector position, 
velocity and acceleration in a fixed 
task-related Cartesian frame of reference 

F 

= nxl vector of “virtual” Cartesian forces 
applied to the end-effector as the 
control input 

M(X) 

= nxn symmetric positive-definite Cartesian 
mass matrix 

N(X,X) 

nxl Cartesian Coriolis and centrifugal 
force vector 

G(X) 

= nxl Cartesian gravity loading vector 

H(X) 

= nxl Cartesian friction force vector 

f 

= nxl vector of forces and torques exerted 
by the end-effector on the load. 


In the above-noted example the load is the shared 
common load 610' between the dual-arm system of 
FIGS. 5 and 7. 

The force/torque vector f both imparts motion to and 
applies force/torque on the load 610' and acts as the 
coupling element between the two arms 600 and 660. In 
the following analysis, the force/torque vector f will be 
considered as a "disturbance input” (analogous to my 
earlier-described auxiliary input signal d(t), C p and Cj) 
to the position control system. The function of the con- 
trol system is to ensure that the end-effector position 
vector X tracks the nxl vector of desired trajectory 
X ^ despite the disturbance force f. For each manipulator 
arm, let us apply the linear adaptive position control 
law described in my earlier application or its corre- 
sponding paper which is identified as H. Seraji, Direct 
Adaptive Control of Manipulators in Cartesian Space, 
Journal of Robotic Systems, February, 1987. Such a 
position control law is: 

F{t) = 40 + [Xft)E(t) + KMm) + [C(0X40 + (102) 

B«)XdM + AiOXdd)] 

as shown in FIG. 3, where E(t)= X^t)— X(t) is the 
n X 1 position tracking-error vector. In the control law 
(102), the nXl vector d(t) is an auxiliary signal to be 
synthesized by the adaption scheme, while [KpE+K v E] 
and [CXd+B X^+AX^] are the contributions due to 
the feedback and feedforward controllers respectively. 
Following my earlier-described method, the required 
auxiliary signal and controller gains are updated ac- 
cording to the following adaptation laws: 

, t (103) 

40 = 40) + S, J riOdt + 8 2 *t) 

0 

t (104) 

K p (t) = K p ( 0) + ai J *t)E(I)dt + a 2 dt)E(t) 

0 
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-continued 

t 

KM = KM + Pi S ’i‘)E , (t)dt + PiKtWit) 
0 


and coefficient of stiffness (elasticity) K 0 . In the joint 
(105) space, the dynamic equations of motion for each arm 
which relates the joint torque vector 


C(0 = C(0) + vi / M X<j{t)di + nM x d \t) 

o 


5(0 = 5(0) + yi / lit) X d \t)dt + yi iit) xj(t) 
0 


A(t) = -4(0) + X, / rit) X d '(t)dl + X 2 K0 X d '(t) 
0 


*0 = WpE(t) + WyE( t) 


f r, ) o, 

T = to the joint angle vector 6 — is given by H. 

Ti &2 


10 Seraji, et al., Linear Multivariable Control of Two-Link 
Robots, Journal of Robotic Systems, pp. 349-365, Vol. 


3, No. 4, 1986 discloses as follows 

i\t)=M(e)'e'+N(sf)+H<,e)±}\ev 

where the above terms are defined as: 
Inertia matrix 


is an nXl vector, {8i,ai,/?i,vi,yi, \i) are positive sca- 
lars, {82, 02, P 2 , V2. 72 , ^2} are positive or zero scalars, and 20 
the prim denotes transposition. In equation (109), W p 
and W v are nxn constant weighting matrices chosen by 
the designer to reflect the relative significance of the 
position and velocity errors E and E. It must be noted 
that since we cannot physically apply the Cartesian 25 
control force F to the end-effector, we instead compute 
the nX 1 equivalent joint torque vector T to effectively 
cause this force. Thus, for each manipulator arm (arm 
600 or 660, FIG. 7, for example), the control law in joint 
space is given by 30 

no = aw = f(e){d(t) + Kptmt) + KMm + oio) 

C( t)Xdt) + BWdit) + A(,)Xe(t)} 

35 

where 8 is the nX 1 vector of joint angular positions and 
J(0) is the nXn Jacobian matrix of the manipulator arm. 

Because of the simplicity of the adaptation laws (103) 
through (108), the robot control algorithm can be im- 
plemented using high sampling rates (typically 1 KHz). 40 
In each sampling period (~ 1 msec), the controller gains 
can change significantly; whereas the terms M, N, G, H, 
and f in the robot model (101) cannot change notice- 
ably. As a result, in deriving equations (103) through 
(108), it was assumed that these terms are unknown and 45 
“slowly time-varying” relative to the gain variations as 
such are changed by these adaptation laws. It is seen 
that the inclusion of the disturbance force f in the robot 
model (101) does not affect the controller adaptation 
laws since the change in f over one sampling period is 50 
relatively small. 

The above-noted observation likewise suggests that 
when both manipulator arms are controlled using the 
two independent adaptive position controllers as imple- 
mented by my adaptation laws, we observe that the 55 
end-effectors will track the desired position trajectories 
despite the interaction forces and torques exerted 
through the load. It must be noted that since the force 
on the load is not a controlled variable in this invention, 
this strategy can lead to undesirable load forces when 60 
the position trajectories are not planned in coordination 
or are not tracked closely. The position-position control 
strategy is illustrated by the following example. 

Consider the planar dual-arm manipulator in a hori- 
zontal plane (g=0) shown in FIG. 7, where the two 65 
arms 600 and 660 are identical and each arm has two 
links and an end-effector with negligible inertia. Sup- 
pose that the load is a linear spring of natural length 1 0 


03(a2/2)cos 62 


a\ + G2 C0S $2 a 3' 

<73 + (ai/ 2 )cos 62 <73 


Coriolis and centrifugal torque vector 


A' ( 0,0 = 


— (02sin 61 ) ($1 #2 + Sr/ 2 ) 

(aisin d 2 ) ef /2 


Coulomb and viscous friction torque vector 


V\ 6 \ + tAsgnfSi) 


^302 + t / 4Sgn(02) 


Jacobian matrix 


— /jsin 0) — Asinffli + 02) — /2Sin(0j -(- 0i) 

/)COS 01 + /2COS(0[ + 02) /2COs(01 + 02) 


In the above expressions, at, .... as are constant param- 
eters obtained from the masses {mi, m2} and the lengths 
{li,l2} of the links, {vi,vj} and {v2,V4} are coefficients 
of viscous and Coulomb frictions respectively, and the 
links are uniform with centers-of-gravity located at 
mid-lengths. The interaction force vector f is modelled 


Ko(l - /^cosa 
ftS - /„)sina 


/ = N (*2 - x\Y + ta - yir 

is the instantaneous length of the coordinates of each 
end-effector given by 

x=d 0 +I\ cos 0i + /2(0i + 02) 
y=l] sin 0) + h sin(0] + 02) 
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-continued 


9 

and d 0 is the base distance from the origin and a is the 
angle between the spring and the x-axis, that is 

CWMWOXM + AMM) 


n - y\ 


The mass of the spring is assumed to be negligible com- 
pared to the masses of the links and hence the force in 
the spring dominates over the inertial force required to 
move the spring. 

For the particular system under study, the numerical 
values of the parameters are chosen as 


mi = 15.91 kg; m2 — 11.36 kg; li = I2 = — 432. m 
Vj = 6.0 Nt. m/rad. sec ~ J , V3 = 4.0 Nt.m/rad.sec -1 
v 2 = V 4 - 1.0 Nt.m; K 0 = 1000.0 Nt./m 


The natural length of the spring is set equal to the initial 
distance between the end-effectors; i.e. 


5 where E(t)=X<Xt)-X(t) is the position tracking-error, 
X(t) = [x(t),y(t)]', and X^t)=[xd(t),y I Xt)u'. For both 
arms, the terms in the control law (112) are adapted as 
follows: 


d(t) = 0.5 tit) + 0.5 / r(l)d< 
0 


Kft) = 2 / iit)E{i)dt 
15 0 


KvM = 2 / d<)EXt)d 
0 


C(/) = 0.5 / K0 Xd'(')dt 
0 


k ’ (xv - xu ) 2 + O2/ - yn ) 2 

so that there will be no initial force in the spring. The 25 
masses and lengths of the links are those of links 2 and 
3 of the Unimation PUMA 560 arm. It must be empha- 
sized that the mathematical model and the parameter 
values of the arms and the spring are used merely to 
simulate the system behavior and are not used to gener- 
ate the control action. This invention does not depend 
upon knowledge of the complex mathematical model of 
the arm dynamics or any knowledge of the arm dy- 
namic parameters or the load parameters such as mass 
or stiffness. As was noted hereinbefore, the lack of re- 
quirement of such knowledge must be acknowledged as 
being one of the key features that contributes to the 
simplified nature of this adaptive hybrid system inven- 
tion and its novel force and/or position control laws, or ^ 
algorithms. 

Suppose that the end-effector of the right arm is re- 
quired to track the desired position trajectories 

x r Jii) — 0. 864[ 1 4- 3e.xp( — r/0.3) — — f/0.4]m. 

45 

y rd (t) =0.m[3exp(- r/0.3) -Aexp( - r/0.4]m. 

to move from the initial point {0,0,-0.864} to the final 
point {0.864,0.0} on the straight line 
y r d(t)=x r d(t)— 0.864. The end-effector of the left arm is 50 
required to track the desired position trajectories 

= 2.0 + 0.432[1 + 3exp( - //0.3)- 
— 4exp(— t/0A)]m. 

55 

yid)t)-0A22[—\+3exp(—l/03)—4exp(-lA)A)]m. 

to move from the initial point {2.0,-0.864} to the final 
point {2.432,-0.432} on the straight line 
yid(t)=xij(j)— 2.864. In this physical set-up the bases of gQ 
the two arms are 2.0 meters apart, and the desired posi- 
tion trajectories for the right and left arms were chosen 
such that appreciable forces were created in the spring. 

The two arms, in accordance with my invention, are 
controlled by identical and independent adaptive posi- 55 
tion control laws given by 

no = S(e)ld(t) + Kp(t)E(t ) + KMm + 


m = 0.5 / *0 x d \t)dt 

0 

t 

A(t) = 0.5 / *0 X d \t)dt 

0 

where 

tit) = 8000£(/) + 800£(/) 


Note that the initial values of the controller terms are all 
chosen arbitrarily as zero. A simple trapezoidal rule is 
used to compute the integrals in the adaptation laws 
with dt= 1 msec. 

To evaluate the performance of the position-position 
control strategy, the nonlinear dynamic models of the 
arms (Equation 111) and the linear adaptive control 
laws (Equation 112) are simulated on a DEC-VAX 
11/750 computer with the sampling period of 1 msec. 
The results of this simulation are shown in FIGS. 9(a) 
through 9(f). FIG. 9(a) shows the magnitude of the 
interaction force 

fm — ^ fx~ + if 

acting on the end-effectors through the spring. It is seen 
that f m changes from the initial value of zero to the final 
value of 373 Newtons in 2 seconds. FIGS. 9(b) through 
9(e) show that the end-effector coordinates x(t) and y(t) 
of both arms track their corresponding reference trajec- 
tories x^(t) and yd(t) very closely despite the large inter- 
action force f m . The paths followed by the end-effectors 
in the horizontal plane are shown in FIG. 9(f). Note that 
both end-effectors track the straight lines as desired. 

We conclude that the adaptive controller for each 
arm performs remarkably well in faithful position track- 
ing despite very large unknown and time-varying inter- 
action forces between the arms. 

SECTION 2- POSITION-HYBRID 

In this section, the position-hybrid control strategy 
for dual-arm manipulators will be studied in which the 
left arm is in pure position control and the right arm is 
in hybrid position/force control, as shown in one em- 


(H2) 
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bodiment of FIG. 3. In other words, for the left arm 
600, FIG. 7, the end-effector position is required to 
track a desired trajectory in a frame of reference. For 
the right arm 660, FIG. 7, in the same reference frame, 
the contact force between its end-effector 665 and the 5 
load 610' must be controlled in the directions con- 
strained by the load 610', while the end-effector 665’s 
position is to be controlled simultaneously in the free 
directions. This control strategy is also applicable when 
one robot arm is confined for operation only in a posi- io 
tion control mode whereas, simultaneously, the other 
arm can be controlled in hybrid control mode. 

For the left arm 600, the interaction forces and torque 
exerted through the load 610' are considered as “dis- 
turbances,” and the adaptive position control system 15 
can ensure tracking of the desired position trajectories 
despite such disturbances, as outlined in Section 1. The 
adaptive position control law for the left arm shown in 
FIG. 7 is given by Equation (37) in my earlier applica- 
tion and may be applied here as 

W) = Wt){d(i) + K^t)E(t) + KME{1) + C(t)Xid(t ) + (113) 

B(l)X ld (t) + A(t)Xtd(t)} 

where T / is the n X 1 joint torque vector, 0 / is the n X 1 
joint angle vector, J i{0/) is the nXn Jacobian matrix, 
X/a(t) — X(t) i the nXl position tracking-error vector 
and the terms in Equation (113) are adapted as follows: 
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tion is to be controlled. In the hybrid control architec- 
ture (as noted in the aforesaid Raibert and Craig and 
Mason articles), two separate controllers may be em- 
ployed for simultaneous force and position control. The 
“force controller” achieves tracking of desired force 
setpoints in the constraint directions; while the “posi- 
tion controller” accomplishes tracking of desired posi- 
tion trajectories in the free directions. 

The dynamic model of the right arm in the constraint 
directions can be written as 

A(X, X)'m + B(X.X)f\t) ~m + C p (Y)±f z =F/t) (121) 

where F z is the mxl “virtual” Cartesian force vector 
applied to the end-effector in the constraint directions, 
Z is the mX 1 vector of end-effector position, the mxm 
matrices A and B are highly complex nonlinear func- 
tions of the end-effector position X, C p is the cross-cou- 
pling from the position loop into the force loop and f z is 
the component of the force exerted on the end-effector 
by the load 610' in the constraint directions. The term f z 
represents the cross-coupling that exists between the 
arms 600,660 through the load 610' and is considered as 
a “disturbance” to the hybrid controller. 

25 An adaptive force control scheme is developed 
within the hybrid control architecture as claimed 
herein. For the right arm, 660, the linear adaptive force 
control law in the constraint directions is given by 
Equation 122 below as 


d(t) = 4(0) + 8 , f ADdt + 82 Kl) 
0 


K p (t) = K/0) + a, / rit)E(l)dt + a 2 rit)E(t) 
0 


KM = KAO) + fit f iii)E(t)dt + 0 2 ME(l) 
0 


C([) = C( 0) + v, / r(,)Xi<At)dt + v 2 MXiM 
0 


B{t) = B(0) + y\ / MX uA‘)dt + y 2 M XldO) 

0 


Ad) = .4(0) + Xi / MX/dit) dt + -hiMXiM 

0 

where 

K0 = WpE{t) + WyE(i) 


(114) 


015) 35 


(116) 


40 


(117) 


(118) 


(119) 


( 120 ) 


and the symbols are defined in Section 1. 55 

We shall now discuss the hybrid position/force con- 
troller for the right arm 660, FIG. 7. Consider a task- 
related “constraint frame” (coordinate system) which is 
defined by the particular contact situation occurring 
between the right end-effector 665 and the load 610'. In 60 
this frame, the n degrees-of-freedom (or directions) in 
the Cartesian space {X} can be partitioned into two 
orthogonal sets; the m constraint directions in subspace 
{Z} and the 1 free directions in subspace {Y}, with 
n=m+l. In the m constraint directions, the end-effec- 65 
tor makes contact with the load 610' and the contact 
force needs to be controlled. In the 1 free directions, the 
end-effector is free to move and the end-effector posi- 


FM = PM + d(t) + KpO)E(t) + KM I E{t)dt - KAt)Z(l) 


where P,(t) is the desired contact force on the load 610' 
used as a feedforward term, d(t) is an auxiliary signal, 
E(t)=Pr(t)—P(t) is the deviation of the actual force P(t) 
from the desired value, and { K p {t),Kj(i),K v (i )} are 
adaptive gains of the PID controller. The terms in the 
force control law, Equation (122), are adapted as fol- 
lows: 


45 


50 


d( t) = 4(0) + 81 / q(l)dt + S 2 ? (f) 
0 


KM = KM) + ai / qd)E*(t)dt + a 2 qO)E r (t) 
0 


Kp(0 = Kp(0) + 0, / q(t)E\t)d, + p iq (t)E(t) 
0 


KM = .MO) + 71 / q(!)Z'(r)dt + 72 MZ'O) 

0 

where 

q(t) = W, E\i) + WpE(t) - wjAl) 


(123) 


(124) 


(125) 


(126) 


(127) 


In equations (123) through (127), 


E\t)= / E(i)dl 
0 

is the integral error vector, {Si,ai,y3i,yi) are positive 
scalars, { 82 , 0 - 2 , 02 , 72 } are positive or zero scalars, and 



5,023,808 


13 

{W/.W^jWv} are constant weighting matrices chosen 
by the designer to reflect the relative significance of E*, 
E and Z. 

The dynamic model of the right arm in the free direc- 
tions can be written as 

no -t -Bj,x.x) no + c^x.x) no + c/a- 

±fy=FJ. 0 (128) 

where fj, is the component of the end-effector force in 
the free directions, C/is the cross-coupling from the 
force loop, A 0 , B„, C 0 are complex nonlinear matrices, Y 
is the end-effector position vector and Fis the “virtual” 
end-effector control force. For the right arm, the linear 
adaptive position control law in the free directions is 
given by 


14 

on the load. This problem is similar to pulling out a 
drawer using two arms. 

The x-component of the force on the load 610' can be 
written as 
5 

fx=k 0 V.X 2 -xi)-l 0 ] 

where (x 2 — xi) is the distance between the end-effec- 
tors, l c is the length of the load, K 0 is the stiffness of 
10 force/torque sensors mounted on the end-effectors to 
measure the force f*. The y-component of the force on 
the load is 


F y(t) = d(t) + Kfi)Efi) + (129) 

kmeji) + amt) + mm + mm 

20 

as in Section 2, where R is the desired position trajec- 
tory E P =R-Y is the position tracking-error, and F^ is 
the “virtual” Cartesian force in the free directions. 
Thus, in order to implement the force and position 
controllers in accordance with Equations (122) and 25 
(129) in the hybrid control architecture, the joint space 
control law for the right arm is given by 


tm = 


' FM 


\ 

/ 


(130) 30 


where 6 r is the joint angle vector, T, is the joint torque 
vector, and 3 r is the Jacobian matrix of the right arm 35 
with appropriate reordering of columns of J r if neces- 
sary. 

The hybrid controller adaptation laws, Equations 
(103) through (108) and (123) through (126) are ex- 
tremely simple, and therefore the control algorithm can 40 
be implemented using high sampling rates (rates (si 
KHz); yielding improved performance particularly in 
force control applications. Since in each sampling per- 
iod (slm sec) the terms in the robot models (121) and 
(128) cannot change noticeably, it is reasonable to as- 45 
sume that these terms are “slowly time-varying” com- 
pared to the adaptation scheme. Thus the inclusion of 
the disturbance f 2 and { y in the robot models (121) and 
(128) does not effect the controller performance. 

It is concluded, therefor, that using the position- 50 
hybrid control strategy, the left end-effector will track 
the desired position trajectory despite the interaction 
forces through the load. The right end-effector will 
exert the desired force on the load in certain directions 
while simultaneously tracking the desired position tra- 55 
jectory in the orthogonal directions. It must be noted 
that in this control strategy, slight fluctuations may be 
observed on the load force due to very small vibrations 
of the left arm under position control. The position- 
hybrid control strategy is now illustrated by the follow- 60 
ing example. 

Let us consider the dual-arm manipulator discussed in 
Example 2 (with negligible friction) and suppose that 
the end effectors carry a rigid load in a horizontal plane, 
as shown in FIG. 7. Suppose that the load 610', as 65 
shown, is initially parallel to the x-axis and we wish to 
make a parallel translation of the load 610' while the 
right arm, 660, applies a constant desired contact force 


where the arms are assumed to share the load equally; 
i.e. each arm “sees” the load mass as m/2. In the simula- 
tion, we take K 0 = 1000.0 Nt/m and m= 1.0 Kg. 

Suppose that the end-effector 605 of the left arm 600 
is required to track the desired position trajectories 

jc/^r) =0.432 meter 

0.432 [1 + 6exp(— r/0.3) — %exp( — r/0.4)] meter 


to move from the initial point {0.432, —0.432} to the 
final point {0.432, 0.432} on the vertical line 
x«(t) = 0.432. The end-effector 665 of the right arm 660 
is required to apply a specified force setpoint P r d on the 
load 610' in the x direction and simultaneously track the 
desired position trajectory y^t) in the y-direction; 
where 

P r d= 10 Newton 

JVd(0=0.432[l + 6exp{— f/0.3) — 8exp( — r/0.4)] meter 


so that the end-effector moves from the initial point 
{2.432, 0.432} to the final point {2.432, 0.432} on the 
vertical line x r d(t)= 2.432, where 2.0 meter is the dis- 
tance between the bases of the two arms. 

The adaptive position control law for the left arm is 
given by 

m = J/(6,)(d(l ) + K p (t)E(t) + KME(t) + (131) 

aoxidio + Bmm + awim} 

where E(t)—Xid(t)—Xi{t) is the position tracking-error, 
-W0=[*/(0> J’XO]'. Xid(t)—[xid<,i), yid(i)]‘ and the terms 
in equation (131) are: 


3(0 = 0.57(1) + 0.5 / 


7(t)dt 


XpW = 2 
KM = 2 

C(r) - 0.5 
B(t) = 0.5 


0 


/ di)E(t)dt 
0 


/ 7U)E(t)di 
0 


/ 7(t)XidV)dt 
0 


/ 7(t)Xi d \t)dt 
0 
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-continued 
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-continued 


m = o.5 ; Kt)x u \t)dt 

0 


where 


CO) = 0.5 / rO)yMdt 
0 


BO) = 0.5 / rO)yMdt 
0 


K0 = 20000£(?) + 2000E(r) 


10 


AO) = 0.5 / rO)yrO)dt 
0 


Note that the initial values of the controller terms are 
set to zero. The integrals in the control law (131) are 
computed using a simple trapezoidal rule with dt=0.5 
msec. 

The adaptive hybrid position/force control law for 1 
the right arm is given by 


tm = Jr(e r ) 


' F x 0) 'j 

J 


(132) 

20 


where 


25 

F x 0) = F rd(j) + dO) + F j{t)e* x 0) + Kp0)‘‘x0) Fjj)x(l) 

Fyil) = do) + Fp0)?yO) + Kd.l)CyO) + 

C0)yM + BOYyM + A0)y'M 

30 


e x (t) = P r Jo ) — Pnff) = force tracking-error 


e/t) = y rs j{i) — yM = position tracking-error 


t 

e*0.i) = / e x t)dl = integral force error 
0 


The adaptation laws for the force controller are: 


dO) = q(t) + J q(t)dt 

o 


40 


where 

r(r) = lOOOe/r) + 500 e/t) 

The integrals in the above adaptation laws are evalu- 
ated using the trapezoidal rule with dt=0.5 msec. 

To evaluate the performance of the position-hybrid 
control strategy, the nonlinear dynamic models of the 
arms (Equation 111) and the linear adaptive control 
laws (Equations 131 through 132) are simulated on a 
DEC-VAX 11/750 computer with the sampling period 
of 0.5 msec and the simulation results are shown in 
FIGS. 10(a)- through 10(6). These Figures show that 
the end-effector coordinates x/(t) and y/(t) of the left 
arm track the desired position trajectories x/ f /t) and 
yw(t) very closely. FIGS. 10(c) through 10(<f) indicate 
that the right end-effector exerts the desired force of 10 
Newtons on the load in the x-direction and tracks the 
desired position trajectory y r d(t) in the y-direction. No- 
tice that the oscillations in the force response in FIG. 
10(c) are due to the very small variations of the left 
end-effector x-coordinate as shown in FIG. 10(a). 
These oscillations can be reduced by increasing the 
gains of the position controller for the left arm. The 
paths traversed by the end-effectors in the horizontal 
plane are shown in FIG. 10(e). It is seen that the left 
end-effector tracks a vertical straight line, whereas the 
right end-effector moves in initially to produce the 
desired contact force and then tracks a vertical straight 
line. Thus, the adaptive position and hybrid controllers 
for each arm perform well in the dual-arm situation. 


AtM = 100 + 100 / q(t)e*x(t)dt 
0 


K/t) =100+10 / q(t)e\0)dt 
0 


AT/f) = 5000 - 40000 / q(')*0)dt 

0 


where 


<K0 = I0e x (0 - I00*(i) + lOeJtft) 


The adaptations laws for the position controller are: 


d0) = 0.5 r(t) + 0.5 / Hf)dt 
0 


SECTION 3- HYBRID-HYBRID 

In this section, the hybrid-hybrid control strategy for 
dual-arm manipulators will be studied in which both 
arms are in hybrid position/force control. In other 
words, in a common frame of reference for both arms, 
the forces exerted by the end-effectors on the load in the 
constraint directions {Z} must be controlled; while 
simultaneously the end-effectors are required to track 
desired position trajectories in the free directions {Y}. 
Any unwanted forces and torques on the load generated 
by the relative position and orientation of the end-effec- 
tors will act as “disturbances” and the adaptive hybrid 
controllers ensure that the desired position/force trajec- 
tories are tracked despite such disturbances. 

Following Section 2, for each manipulator arm the 
hybrid position/force control law in the joint space can 
be written as 


45 


50 


55 


60 


KpO) = 2 S 

0 


rO)e/l)dt 


•K/O = 2 / r(i)e/.t)dt 
0 


where J(0) is the Jacobian matrix (with appropriate 
column reordering if necessary), and F^t) and F^(t) are 
the “virtual” Cartesian forces applied to the end-effec- 
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tor in the constraint directions {Z} and free directions 
{Y}, respectively. The force control law is given by 


FM = PM + 4(1) + KM f EMdt + 
0 


EJAEM - KMZ(t) 


where Pit) is the desired force setpoint, 10 
EXt)=PXt)-Pz(t) is the force tracking-error and the 
adaptation laws are: 


d(t) = 4( 0) + 8] I qMt + s 29(0 
0 


KM = KAO) + a 1 / q(t)E*z(t)dl + aiMEA(<) 

0 


K p 0) = Kpt 0) + /3i / qU)E z X0dt + Piq(t)E z '(t) 
0 


*vW = /T^O) = 7 , I qiAZ'{t)dt - y 2 q(t)ZV) 
0 


-continued 

ho = + >n4(/) 

and {Wp,W v } are desired weighting matrices. 

The above controller adaptation laws are extremely 
simple and therefore the hybrid control algorithm can 
be implemented using high sampling rates (~1 KHz); 
yielding improved performance. Under the adaptive 
hybrid controllers, both end-effectors are expected to 
exert the desired forces on the load while simulta- 
neously moving on desired trajectories. The hybrid- 
hybrid control strategy is most available when simulta- 
neous control of both position and force is required. 
The following example illustrates the hybrid-hybrid 
control strategy. 

Consider the dual-arm manipulator and load, as dis- 
cussed above in conjunction with FIG. 7, and suppose 
that both end-effectors are required to exert a constant 
desired force on the load in the x-direction while mov- 
ing the load in the y-direction. In this situation, the x 
and y components of the load force are the same as 
developed in Section 2. 

Suppose further that the desired force and position 
trajectories for both arms are specified as 

Pxdi0 = — IOjWh 'ton 


q{t) = W,EM + WpEM - W,Z( 0 


EA0 = / EMdt and { W t , Wp W v } 

0 

are desired weighting matrices. 

The position control law is expressed as 

FM = 4(1) + K p U)E/0 + 


7<X0 =0.432[l+6 exp( — 1/0.3)— 8 exp( - l/0.4)] meIe , 

so that the end-effectors move from the initial points 
{[0.432, —0.432], [2.432, —0.432]} to the final points 
{[0.432, 0.432], [2.432, 0.432]} while exerting a force of 
10 NT on the load. 

The two arms are controlled by identical and inde- 
pendent adaptive hybrid control laws given by: 


(135) 40 


KME/t) + C(0m + BV)k(t) + AU)R(0 

where R(t) is the desired position trajectory, 
E^t) = R(t) — Y(T) is the position tracking-error and the ^ 
adaptation laws are: 


4(l) = 4(0) + h / HOdt + 8 2 H0 
0 


Kp{0 = KjW) + v, / HO Ey'iOdl + v 2 H0 EyXO 
0 


no = f(0) 


(FM \ 

) 


FM = PrM + 4(1) + KMe’M + KpiOeM - 
KMkO 

FM = 4(1) + KM*M + KM e/t) + C(0yM 
+ B(0i>M + A(t)yM 

e x (t) = P r d(0 — P.xi?) — force tracking error 
eft) = y r d(0 — yM — position tracking-error 


e^(t) = f €x(t)dt = integral force error 
0 


km = kx 0) + m f Kb £>'(04i + mHOFM 
o 


C(i) = C( 0) + ?i / HORXOdt + ihHWXO 

o 


B(t) = 5 ( 0 ) + 71 / HORXOdt + hHORXO 
o 


A(t) = 1(0) + X, / HORXOdt + hHORXO 
0 


The adaptation laws for the force controller are: 


4(1) = 9 ( 1 ) + / q(t)dt 
0 


KM =100+10 / M&Odt 
0 


KM =100 + 4 / q0)eMdt 
0 


KM = 5000 - 1000 / qiOMdt 
0 


where 
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-continued 


where 

9(0 = <?jr(0 - 10 jc(/) + e'M 

The adaptation laws for the position controller are: 


i 


<7(0 = 0.5*0 + 0.5 I r(t)dt 
0 

fpC 0 = 2 

t 

/ MeM dt 
0 

KM = 2 

/ ' K0f/0* 
0 

C( 0 = 0.5 

I MyMdt 
0 

B(l) = 0.5 

t 

f MyMdt 
0 

1(0 = 0.5 

I MyMdt 
0 

where 


*0 = 5000 

e/t) + 2000 e/,t) 


The integrals in the above adaptation laws are evaluated 
using the trapezoidal rule with dt=0.5 msec. 

To evaluate the performance of the hybrid-hybrid 
control strategy, the nonlinear dynamic models of the 
arms (Equation 111) and the linear adaptive control 35 
laws (Equation 136) are simulated on a DEC-VAX 
11/750 computer with the sampling period of 0.5 msec 
and the simulation results are shown in FIGS. ll(a)-(<f)- 
FIG. 11(a) shows that the force exerted by the end- 
effectors on the load in the x-direction is equal to the 40 
desired setpoint of 10 Nt. FIGS. 11(6) through 11(c) 
indicate that the coordinates of the end-effectors in the 
y-direction track the desired position trajectories. The 
paths traced by the end-effectors in the horizontal plane 
are shown in FIG. 11(d). It is seen that both end-effec- 45 
tors move in initially to produce the desired force and 
then track vertical straight lines. Thus the adaptive 
hybrid controllers perform extremely well for simulta- 
neous force and position control. 

Three adaptive control strategies for cooperative 50 
dual-arm robots have been described. In these strate- 
gies, each robot arm is considered a subsystem of the 
total system and is controlled independently using an 
adaptive controller in the low level of the control hier- 
archy. Each controller ensures that the controlled vari- 55 
ables follow desired commands and reject unwanted 
cross-coupling effects from other subsystems which are 
treated as “disturbances.” The subsystems are coordi- 
nated through trajectory generators in the intermediate 
level, where synchronous desired trajectories for both 60 
arms are specified in a common task-related frame of 
reference. An important feature of the present approach 
is that the overall control system for N cooperative 
arms is reduced to N decentralized independent single- 
arm controllers. The control schemes do not require 65 
communication and data exchange among individual 
controllers, which is an appealing feature from both 
computational and reliability points of view. Further- 
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more, available techniques for single-arm control can be 
utilized directly in multiple-arm environments. 

The control strategies described herein do not require 
the knowledge of the load parameters such as mass and 
stiffness or the robot dynamic parameters such as link 
masses and inertias, and can therefore cope with uncer- 
tainties or variations in the system parameters. Further- 
more, the complex dynamic model of the arms are not 
used in generating the control actions. The control 
schemes are very simple and extremely fast for on-line 
implementation with high sampling rates, yielding im- 
proved dynamic performance. The control methodol- 
ogy described herein can also be utilized in the coordi- 
nated control of N-arm robots when N exceeds two. 

APPENDIX 

In this Appendix, we obtain a simple expression for 
the force on a rigid load held by two end-effectors in a 
horizontal plane. 

Let us consider a rigid load of length l 0 held firmly by 
two end-effectors equipped with force/torque sensors 
as shown in FIGS. 7 and 8. The sensors are modelled as 
linear springs with stiffness coefficients Ki and K .2 and 
natural lengths li and h. The forces Fi and F 2 exerted by 
the sensors on the rigid load are given by 


Fi = KMl-h) 

(137) 

II 

x 

(138) 


where l'i and l^are instantaneous lengths of the springs. 
At equilibrium, the forces Fi and F 2 must be equal; 
otherwise the load will reposition itself under the net 
force to reach the equilibrium condition. Let us denote 
the force exerted by the springs on the load by 
F=Fi=F 2 ; hence 

F=Fl=JCl[/'l-/l+/ 0 + /0 + /'2-/'2 + /2-/2] (139) 

where equal and opposite terms are added in equation 
(139). From equation (139), we obtain 


F=K l [L'-L-(l'2-l2)] = K l (L'-L)-K ] (r 2 -l2) (140) 


where 

L' = /'l H-/o +/' 2 — instantaneous distance AB 

L' = /i + /o + /2 = “natural” distance AB with no force 
on the load, i.e. (AB)o 


Using F=F 2 =Ki(l' 2 —li), equation (140) becomes 


F = 


K\(L' -L)-Ki 



(141) 


K\K 2 

F = Ki + k 2 ' [£’ - L] = F eq [L - L) = K eq [AB - (AB) 0 \ 

where K eq is the equivalent stiffness coefficient of the 
springs obtained from 

“ = ~ + ~k~' 

Equation (141) gives a single expression for the force on 
the load at any instant time. 
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The above description presents the best mode con- 
templated in carrying out the invention. The invention 
is, however, susceptible to modifications and alternate 
constructions from the embodiments shown in the 


22 

desired position response that is indicated by said 
position command signal. 

5 . The hybrid controller in accordance with claim 4 
and wherein the improvement is further characterized 


drawings and described above Consequently, it is not 5 by: 

the intention to limit the invention to the particular each manipulator’s adaptive control means is inde- 
embodiments disclosed. On the contrary, the invention pendently operative from the adaptive control 

is intended to and shall cover all modifications, sizes and means which control the other manipulators; and 


alternate constructions falling within the spirit and means for issuing coordinated force and/or position 
scope of the invention, as expressed in the appended 10 commands to each of said independently operative 
claims when read in light of the description and draw- adaptive control means. 

ings. 6. The hybrid controller in accordance with claim 5 

What is claimed is wherein said adaptive control means is further charac- 

1. In a hybrid controller adapted to issue control terized as comprising: 

signals, each of which control a respective manipulator 15 an adaptive force control circuit which includes a 
in a multi-manipulator system wherein each manipula- force signal feedback and feedforward generating 

tor’s position and/or force is applied in controlling a means; 

common load being acted upon in coordination by all of an adaptive position control circuit which includes a 
the manipulators of the system, the improvement com- position signal feedback and feedforward generat- 

prising: 20 j n g means; and 

means individually associated with each of said ma- means for connecting both said force and position 
nipulators for independently coordinating its part signal generating means to said manipulator driv- 

in controlling said load, and in so doing, introduc- mg means in order to control said manipulator’s 

ing into the other manipulators, through the load, a position and/or force exerted on said load in accor- 

cross-coupled position and/ or force term; and 25 dance with said position and/or force commands, 

adaptive control means for generating a variable con- 7 \ hybrid controller in accordance with claim 2 
trol signal applied to each of said manipulators, wherein said adaptive control means further comprises: 
which variable control signal compensates in real- means for deriving an auxiliary signal for controlling, 

time for the inter-manipulator cross-coupling terms at least j n part, the position and dynamics of said 

introduced into the other manipulators through 30 manipulator; and 

said load as it is being acted upon by said multi- summing means for adding said auxiliary signal in 

manipulator system. combination with command signals representing 

2. The hybrid controller in accordance with claim 1 position and dynamics in said position-controlling 

wherein the manipulators operate in an environment adaptive feedforward and feedback control loops, 

and such manipulators and the environment form a 35 8 .A hybrid controller in accordance with claim 1 and 

complex, dynamic model specified as having environ- including a position and/or force command generator 

mental and manipulator parameter values that are un- f or emitting signals indicative of a desired position and- 

known, and the improvement is further characterized / or f orce on said load and further wherein each manipu- 

by: lator’s actual force/position applied to said load are 

said variable control signal generating means include 40 sen sed in order to derive position/force error terms that 
signal handling circuitry which is formulated and are thereafter respectively represented as error signals 

operable free from any knowledge of said model by the hybrid controller, and wherein the improvement 

environmental or parameter values. j s further characterized in that said adaptive control 

3. The hybrid controller in accordance with claim 2, means comprises: 

wherein each manipulator exhibits a force response and 45 a s j gna i summing junction for forming said real-time 
includes a driving means that is responsive to a force f orce and/or position control signal(s); 

command signal, and the improvement is further char- a feedforward command control loop for applying 
acterized by: said desired force/position command signal(s) to 

said adaptive control means including at least one sa id summing junction; 

adaptive force controller for controlling each of 50 an adaptive force/position control loop for receiving 
said manipulator’s driving means in the force that it said desired force command signal; and 

applies to the load; and adaptation control means in said adaptive force/posi- 

said adaptive force controller includes a realtime tion control loop responsive to said command sig- 

force signal generating means for emitting a signal nal(s) and also responsive to said actual sensed 

that causes said manipulator to exhibit the desired 55 error signal(s) for modifying the command(s) and 

force response that is indicated by said force com- error(s) signals and applying the so-modified sig- 

mand signal. nal(s) to said summing junction. 

4 . The hybrid controller in accordance with claim 3, 9 a hybrid controller in accordance with claim 8 and 

and wherein each manipulator exhibits a position re- wherein said signal modifying means in said adaptive 
sponse and includes a driving means that is responsive 60 control means operates in accordance with a force con- 
to a position command signal, and the improvement is trol j aw> anc j sa jd controller further comprises: 
further characterized by: a plurality of variable gain circuits for implementing 

at least one adaptive position controller for control- a f orce control law characterized as: 

ling each of said manipulator’s driving means in the 
position that it applies to said load; and 65 

said adaptive position controller includes a real-time FM = PM + d(l) + KM / E «)di + K/t)E(t) - KMZ(t) 
position signal generating means for emitting a o 

signal that causes said manipulator to exhibit the 
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wherein P/t) is said desired force command signal 
term; d(t) is, at least in part, a cross-coupling for- 
ce/position compensating term; the K/, K^, and K v 
signal terms are individually varied gain control 
terms; E is a force error term and Z is a velocity 5 
damping term. 

10. A hybrid controller in accordance with claim 9 
and wherein said .signal modifying means in said adapt- 
ive control means operates in accordance with a posi- 
tion control law, and said controller further comprises: 10 

a plurality of variable gain circuits for implementing 
a position control law characterized as: 

F)Hf) = M + W)EjA‘) + KMEp(t) + 

Cit)R(t) + BWO) + A{t)R(t) 

wherein the term R(t) is a desired position trajectory 
signal term, [K^E^ + KpE/>] and [CR + BR+AR] 
are terms contributed by the feedback and feedfor- 20 
ward adaptive position control loops. 

11. A hybrid controller in accordance with claim 1 
and including a position and/or force command genera- 
tor for emitting signals indicative of a desired position 
and/or force on said load and further wherein each 25 
manipulator’s actual force/position applied to said load 
are sensed in order to derive position/force error terms 
that are thereafter represented by error signals to be 
fedback over a feedback control loop to the hybrid 
controller, and wherein the improvement is further 30 
characterized in that said adaptive control means also 
comprises, in addition; 

a signal summing junction for forming said real time 
force and/or position control signal(s); 

a feedforward command control loop for applying 35 
said desired force/position command signal(s) to 
said additional summing junction; 

an adaptive control loop for receiving said desired 
force command signal; and 

adaptation control means in said adaptive control 40 
loop responsive to said force command signal and 
also responsive to said actual sensed force error 
signal for modifying the command and error sig- 
nals and applying the so-modified signal to said 
summing junction. 45 

12. A hybrid controller in accordance with claim 11 
and further comprising: 

a position/force trajectory generator emitting desired 
force/position trajectory signals including posi- 
tion, velocity and acceleration signals; and 50 

means for applying said desired position, velocity and 
acceleration signals to said adaptive position feed- 
forward and adaptive position feedback loops. 

13. A hybrid controller in accordance with claim 12 

and further comprising: 55 

a linear adaptive position control law represented as 
the output signal from said adaptive position con- 
trolling means. 

14. A hybrid controller in accordance with claim 1 
and wherein said improvement is further characterized 60 
by: 

means for compensating for dynamic cross coupling 
which results from adaptively controlling both 
position and force of said load by said manipula- 
tors. 65 

15. A hybrid controller in accordance with claim 14 
wherein the manipulator driving means is further char- 
acterized as including: 
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a signal summing junction for force control connect- 
able to said manipulator(s) for applying thereto a 
combined variable force control signal derived, at 
least in part, from said feedforward control loop. 

16. A hybrid controller in accordance with claim 15 
and further comprising: 

variable gain control circuits in said adaptive force 
control loop; and 

an adaptation control responsive to signals indicative 
of the manipulator’s actual Cartesian movements in 
said environment for controlling said gain control 
circuits in order to compensate for the system’s 
non-linearities. 

17. A hybrid controller in accordance with claim 16 
and further comprising: 

a position/force trajectory generator emitting force 
reference and/or position trajectory reference 
command signals; and 

means for comparing the desired position/force refer- 
ence command signals) and an actual force signal 
representing the Cartesian force applied by said 
manipulator to said load, in order to derive an error 
signal for application to said feedback control 
loops. 

18. A hybrid controller in accordance with claim 17 
wherein said signal summing means further comprises: 

a signal summing junction for summing said desired 
force reference command signal with said actual 
manipulator Cartesian force signal to derive there- 
from said real-time force control signal. 

19. A hybrid controller in accordance with claim 18 
and further comprising: 

means connecting said feedforward loop between 
said position/force command generator and said 
signal summing junction for feeding said desired 
force reference signal forward to said signal sum- 
ming junction. 

20. A hybrid controller in accordance with claim 15 
and further comprising; 

proportional-integral-differential (PID) controller 
circuits in said feedback and feedforward force 
control loop. 

21. A hybrid in accordance with claim 20 and further 
comprising: 

variable gain control circuits in each of the PID con- 
troller circuits; and 

means for varying said variable gain control circuits 
in order to compensate, at least in part, for said 
system non-linearities. 

22. A hybrid controller in accordance with claim 21 
and further comprising: 

a linear adaptive force control law represented as said 
output signal from said signal summing junction. 

23. A hybrid controller in accordance with claim 22 
wherein said force control law is further characterized 
as: 

t 

FM = PM 4 - d(t) + KM f E(t)dt + Kp(i)E(t) - KM)Z(t) 

0 

wherein P/t) is said desired force reference signal 
term, d(t) is an auxiliary force signal term emitted 
by said adaptation control, and the K/, K p , and K v 
signal terms are varied by the gain control circuits 
for the PID controller circuits in said feedback 
force control circuit. 
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24. A hybrid controller in accordance with claim 1 
wherein said positioning of said load is controlled by a 
control law characterized as: 

5 

FJ. 0 = M + + KMEfi) + 

emu) + ewe) + mm 

wherein the term R(t) is a desired position trajectory, 10 
[KpEp+KpE p } and [CR+BR + AR] are terms con- 
tributed by feedback and feedforward adaptive 
position control loops; and 

wherein said force control law is characterized as: ^ 

wherein P^t) is said desired force reference signal 
term, d(t) is an auxiliary force signal term emitted 
by said adaptation control, and the K/, K p , and K v 
are variable gain terms; E is an error term; and Z is 
a velocity term. 20 

25. A method of cooperative dual-arm robots to- 

gether manipulating, via end-effectors, a common load 
by a position -position and position-hybrid control strat- 
egy, comprising the steps of: 25 

controlling the robots by adaptive controllers; 
commanding both arms of one robot to track desired 
trajectories in Cartesian space despite unknown 
time-varying interaction forces exerted through 
the load; 30 

adaptively controlling, for said robot, one arm’s end- 
effector motions in free directions and applied 
forces in the constraint directions in Cartesian 
space, while 35 
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adaptively controlling the other arm to ensure that its 
end-effector tracks desired position trajectories in 
said space. 

26. A method of control in accordance with claim 25 
wherein said adaptive control includes 

coordinating force/position control over said arms at 
a command level. 

27. A method of control of cooperative dual-arm 
robots together manipulating, via end -effectors, a com- 
mon load by a position-position control strategy, com- 
prising the steps of: 

controlling the robots by adaptive controllers; and, 
for said position-position control strategy, 
commanding both arms to track desired trajectories 
in Cartesian space despite unknown time-varying 
interaction forces exerted through the load, and 
further comprising the steps of 
additionally including a position-hybrid control strat- 
egy, wherein the step of adaptive control further 
includes: 

adaptively controlling one arm’s end-effector mo- 
tions in free directions and applied forces in the 
constraint directions in Cartesian space, while 
adaptively controlling the other arm to ensure that its 
end-effector tracks desired position trajectories in 
said space. 

28. A method of control in accordance with claim 27 
and additionally including a hybrid-hybrid control 
strategy, wherein the step of adaptive control further 
includes: 

ensuring that both controller’s end-effectors track 
reference position trajectories; while, 
simultaneously applying desired forces on the com- 
mon load being acted upon by the dual-arm robots. 
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